package fri.pipt.agent.deathrow.state;

import java.util.ArrayList;
import java.util.Collection;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;

import javax.swing.plaf.basic.BasicInternalFrameTitlePane.MaximizeAction;

import fri.pipt.agent.deathrow.Pos2;
import fri.pipt.agent.deathrow.World;
import fri.pipt.agent.deathrow.path.PathNode;
import fri.pipt.agent.deathrow.path.PathPlanner;
import fri.pipt.protocol.Message.Direction;

public class StateMachine
{
	
	World agentWorld;
	Pos2 agentPos;
	AgentState agentState;
	boolean makeNextMove;
	boolean agentInMoving = false;
	Direction selectedDirection = Direction.NONE;
	
	public StateMachine(World world)
	{
		agentPos = new Pos2();
		agentState = AgentState.none;
		makeNextMove = false;
		agentWorld = world;
	}
	
	/**
	 * 
	 * @return vrne true, če je stroj pripravljen na nasledni move!
	 */
	public boolean makeNextMove() {  return makeNextMove; }
	
	public Direction getDirection()
	{
		return selectedDirection;
	}
	
	public void init()
	{
		agentState = AgentState.init;
	}
	
	/**
	 * Game update!
	 * @param pos
	 */
	public void agentMoved(Pos2 pos)
	{
		this.agentPos.set(pos);
		makeNextMove = false;
		agentInMoving= false; //Ne more biti več v muvingu!
//		if(inMovingTo!= null)
//		{
//			System.out.println("is move cool? " + inMovingTo.equals(pos));
//		}
	}
	
	/**
	 * Izračunamo naslednje stanje agenta z analizo sveta. 
	 * Naslednje stanje računamo samo v primeru, če se je prejšnje stanje že izvršilo 
	 * oz. če je prišlo do situacije, ki prekine trentne akcije.
	 * 
	 * @param world
	 * @param agentPos
	 */
	public void update(Pos2 agentPos)
	{
		switch (agentState)
		{
			case init:
				agentState = AgentState.explore;
				break;
			case explore:
				doExplore(agentPos);
				break;
			case flagFound: //We know the location of flag, go capture it!
				break;
			case flagCaptured: //We have flag, what to do now?
				break;
			case carryFlagToHeadquarters: //Make plan to reach headquarters
				break;
			default:
				System.err.println("State not implemented!");
				break;
		}
	}
	
	public void moveCommandSend()
	{
		makeNextMove = false;
	}
	
	/**
	 * Glede na trenutno opazovani svet poiščemo vse točke, ki so zanimive za 
	 * raziskovanje in vsako točko opremi z razdaljo do te točke od prenutne pozicije!
	 * 
	 * Glede na hevristiko in nek naključni faktor izbere določeno točko, ki 
	 * je zanimiva! 
	 * 
	 * @param positionFrom
	 * @return
	 */
	public Pos2 getNextGoodPosition(Pos2 positionFrom)
	{
		//Naredimo tabelo nodov
		int w = agentWorld.getWidth();
		int h = agentWorld.getHeight();
		if(w == 0 || h == 0)
			return null; //World še ni postavljen!
		PathNode mapPN[][] = new PathNode[w][h]; //Vse je null!
		PathNode rootNode = new PathNode(positionFrom);
		rootNode.obiskan = true;
		PriorityQueue<PathNode> lstCandidates = new PriorityQueue<PathNode>();
		
		PriorityQueue<PathNode> nodes= new PriorityQueue<PathNode>();
		nodes.add(rootNode);
		
		//Se pomikamo po mapi, po dijkstra algoritmu.
		//Ampak ni enaki pogoj za ustavitev kot pr dijkstri v našem primeru,
		//ker ne načrtujem poti, ampak po dijkstri iščem ustrezne lokacije za raziskovanje
		//v bistvu se algoritem konča ko ne morem dodati nobene nove točke v priority queue
		/*
		 * Koraki algoritma:
		 * 1. vzamemo točko P iz vrste ( če ni točke v vrsti končamo algoritem)
		 * 2. poiščemo vse sosednje točke točki P
		 * 2.1  pogledamo če je katera sosednja točka zanimiva
		 *      Kdaj je točka zanimiva?
		 *      -če je točka na robu mape(kar pomeni da eno mesto leve,desno, gor ali dol) in da če 
		 *       pridemo do tega mesta bomo odkrili nove točke ki so lahko prehodne, zanimive ali pa je rob.
		 *      -če je poiskana točka neraziskana točka(če pridemo do tega mesta odkrijemo neodkrit del mape)
		 *      No pol pa še ostanejo nezanimive točke:
		 *      -ja to so pa vseostale
		 * 2.2 Cilj algoritma je poiskati vse zanimive točke in izračunati razdaljo do njih jih 
		 *     dati v nek list za nadalno odločanje.
		 * 2.3 če točka ni zanimiva damo točko na sklad in uprejtamo prednika in razdaljo do točke
		 *     (po dijkstra algoritmu)
		 * 2.4 če je sosednja točka zanimiva pravtako naredimo kar naredimo v 2.3 ampak jo damo na poseben list
		 *     vendar je ne damo na sklad samo v primeru če je zanimiva točka neraziskana točka! Tako preprečimo
		 *     nadaljevanje dijkstre po neraziskanem območju! Neraziskano območje ne vemo kakšno je!
		 * 3. algoritem se konča, ko so vse točke na mapi preiskane oz. je sklad točk prazen!
		 * 
		 * 4. Če v seznamu ni zanimivih točke pomeni da poznamo celotno mapo in da se je treba
		 *    posvetiti drugim ciljem!
		 * 5. če seznamm zanimivih točk ni prazen
		 * */ 
		PathNode top;
		Pos2 selectedPos = null;
		while((top = nodes.poll()) != null)  //Dokler je kaj se ne ustavimo
		{
			//Pogledamo vse 4 sosede
			for(int i = 0; i < World.MOVES.length; i++)
			{
				int nx = World.MOVES[i].x + top.pos.x;
				int ny = World.MOVES[i].y + top.pos.y;
				int ndist = top.dist+1;
				//kako dobiti pravo koordinato?
				int mapx = nx - agentWorld.getObservationArea().minX;
				int mapy = ny - agentWorld.getObservationArea().minY;
				//Ali smo izven dovoljenega območja
				if(nx < agentWorld.getObservationArea().minX || nx > agentWorld.getObservationArea().maxX || 
						ny < agentWorld.getObservationArea().minY || ny > agentWorld.getObservationArea().maxY )
				{
					continue;
				}
				//neobstoječa lokacija
				if(mapPN[mapx][mapy] == null)
				{
					PathNode n = new PathNode(new Pos2(nx, ny), ndist, top);
					n.obiskan = true;
					n.previous = top;
					//Preverimo ali je točka posebna
					//1. ali je robna!
					//2. ali je neraziskana!
					if(agentWorld.getTile(n.pos) == World.TILE_NOT_VISITED)
					{
						lstCandidates.add(n); //ne damo na sklad, ker je kandidat za raziskovanje!
					}
					else if(agentWorld.isWalkable(n.pos))
					{
						mapPN[mapx][mapy] = n;
						nodes.add(n); //damo na sklad ker lahko še obstaja kakšna pot do sem!
						//ali je robna točka?
						if(nx == agentWorld.getObservationArea().minX || nx == agentWorld.getObservationArea().maxX || 
								ny == agentWorld.getObservationArea().minY || ny == agentWorld.getObservationArea().maxY )
						{
							lstCandidates.add(n);
						}
					}
				} 
				else //Ta prostor smo že obiskali! Obstaja alternativna pot do te točke.
				{
					PathNode  n = mapPN[mapx][mapy];
					if(n.dist > ndist) //Obstaja krajša pot!
					{
						n.previous = top;
						n.dist = ndist;
						n.obiskan = true;
					}
				}
			}
		}
		if(lstCandidates.size() == 0)
		{
			//WORLD OWNAGE!
			System.err.println("Vse sem že raziskal in ni nikjer zastvice na poti?! Pol jo more imeti team-mate...? ");
			System.exit(-1);
		}
		else
		{
			//izberemo top 5 in naključno med njimi!
			ArrayList<PathNode> lstPN = new ArrayList<PathNode>();
			for(int i = 0; i < 5 && lstCandidates.size() > 0; i++)
			{
				lstPN.add(lstCandidates.poll());
				System.out.println("Canditate " + (i+1) + " " + lstPN.get(lstPN.size()-1).pos );
			}
			int idxIzbran = (int)(lstPN.size() * Math.random()) % lstPN.size();
			PathNode sPN = lstPN.get(idxIzbran);
			{
				selectedPos = new Pos2(sPN.pos); 
			}
			
			//Rebuild path
			LinkedList<PathNode> path = new LinkedList<PathNode>();
			PathNode tmp = sPN;
			while(tmp != null)
			{
				path.addFirst(tmp);
				tmp = tmp.previous;
			}
			//Debugging only!
			System.out.println("Reconstructed path!");
			for(PathNode fpn : path)
			{
				System.out.print(fpn.pos + " ");
			}
			System.out.println();
		}
		return selectedPos;
	}

	boolean initExplore = false;
	PathPlanner explorePlan = null;
	
	public void doExplore(Pos2 agentPos)
	{
		if(!initExplore)
		{
			System.out.println("Mode explore: init");
			//We have to make a plann for agent 
			makeNextMove = false;
			Pos2 finalPos = getNextGoodPosition(agentPos);
			explorePlan = new PathPlanner();
			explorePlan.planPath(agentWorld, agentPos, finalPos);
			if(!explorePlan.moveToNexPoint())
			{
				//Nič ne naredimo iščemo nasledno dobro pozicijo!
				initExplore = false;
				return;
			}
			initExplore = true;
			Pos2 nextPos = explorePlan.getPointOnPath().pos; //Next position on world!
			putAgentInMoving(getDirection(agentPos, nextPos), nextPos);
			agDir = getDirection(agentPos, nextPos);
		}
		else
		{
			//Plan is created! We have to follow the path of the one!
			//TODO: But before following the plan check if the plan is not
			//interrupted!
			//System.out.println("Exploring path: is agent in Moving? " + agentInMoving);
			if(!agentInMoving) //Kar pomeni da se je agent prstavu kamo smo mu rekli!
			{
				if(!explorePlan.moveToNexPoint())
				{
					initExplore = false; //povemo da ponovno iščemo destinacijo
					return;
				}
				putAgentInMoving(getDirection(agentPos, explorePlan.getPointOnPath().pos), explorePlan.getPointOnPath().pos);
				agDir = getDirection(agentPos, explorePlan.getPointOnPath().pos);
				System.out.println("Mode explore: path following -> putting agent in moving from " + agentPos + " to " + explorePlan.getPointOnPath()  + " agDir " + agDir);
			}
		}
	}
	Direction agDir;
	Pos2 inMovingTo;
	
	private void putAgentInMoving(Direction dir, Pos2 to)
	{
		System.out.println("Put agent in moving: " + dir + " from:" + agentPos + " to " + to);
		this.inMovingTo = to;
		selectedDirection = dir;
		makeNextMove = true;
		agentInMoving = true;
	}
	
	private static Direction getDirection(Pos2 from, Pos2 to)
	{
		int dx = to.x - from.x;
		int dy = to.y - from.y;
		if(dx > 0)
		{
			return Direction.RIGHT;
		} else if(dx < 0)
		{
			return Direction.LEFT;
		}
		if(dy > 0)
		{
			return Direction.DOWN;
		} else if(dy < 0 )
		{
			return Direction.UP;
		}
		
		return Direction.NONE;
	}
	
}
